#ifndef LOCALIZATION_LOCALIZATION_H_
#define LOCALIZATION_LOCALIZATION_H_

#include <core/macros.h>
#include <core/synced_current_covariance.h>
#include <core/synced_current_pose.h>
#include <localization/dynamics_model.h>
#include <localization/vision_model.h>

namespace Localization {
const double kPi = 3.14159265358979323846;

double AdjustAngle(double a);
}

class LocalizationEstimator {
public:
  // An initial pair of calls to the Read() method of the PlayerCc::PlayerClient object
  // must have already been made by the time this constructor is invoked. Should this
  // condition not be satisfied, proper behavior of objects of this class cannot be
  // guaranteed.
  LocalizationEstimator(const player_pose2d *initial_mean,
          const gsl_matrix *initial_covariance,
          const std::vector<player_point_2d> &mapped_lamps,
          PlayerCc::PlayerClient *player_client,
          PlayerCc::Position2dProxy *position2d_proxy,
          SyncedCurrentPose *current_pose, SyncedCurrentCovariance *current_covariance,
          SyncedLampCentroids *lamp_centroids);
  ~LocalizationEstimator();

  void operator()();
private:
  static const int kSleepTimeInMilliseconds = 20;
  static const bool kDebugMode = false;

  player_pose2d *mean_;
  gsl_matrix *covariance_;

  SyncedCurrentPose *current_pose_;
  SyncedCurrentCovariance *current_covariance_;

  DynamicsModel dynamics_model_;
  VisionModel vision_model_;

  PlayerCc::PlayerClient *player_client_;

  DISALLOW_IMPLICIT_CONSTRUCTORS(LocalizationEstimator);
};

#endif
